//! Time utility functions for ROS2 TimeSpec handling
//! 
//! This module provides utilities for converting ROS2 TimeSpec messages
//! to human-readable timestamps and other time-related operations.

use chrono::{DateTime, Utc};

/// Convert ROS2 TimeSpec to human-readable timestamp string
/// 
/// TimeSpec contains sec (int32) and nanosec (uint32) fields representing
/// seconds and nanoseconds since Unix epoch.
/// 
/// # Arguments
/// * `stamp` - Reference to ROS2 TimeSpec message
/// 
/// # Returns
/// * `String` - Formatted timestamp string in UTC format
/// 
/// # Examples
/// ```
/// use unitree_ros2_rs::utils::time_utils::timespec_to_datetime;
/// 
/// // Create a sample TimeSpec
/// let mut timespec = r2r::unitree_go::msg::TimeSpec::default();
/// timespec.sec = 1640995200; // 2022-01-01 00:00:00 UTC
/// timespec.nanosec = 0;
/// 
/// let timestamp_str = timespec_to_datetime(&timespec);
/// println!("Message timestamp: {}", timestamp_str);
/// ```
pub fn timespec_to_datetime(stamp: &r2r::unitree_go::msg::TimeSpec) -> String {
    // Convert seconds and nanoseconds to DateTime using the new API
    match DateTime::from_timestamp(stamp.sec as i64, stamp.nanosec) {
        Some(datetime) => {
            datetime.format("%Y-%m-%d %H:%M:%S%.3f UTC").to_string()
        }
        None => {
            format!("Invalid timestamp: sec={}, nanosec={}", stamp.sec, stamp.nanosec)
        }
    }
}

/// Convert ROS2 TimeSpec to DateTime<Utc> object
/// 
/// # Arguments
/// * `stamp` - Reference to ROS2 TimeSpec message
/// 
/// # Returns
/// * `Option<DateTime<Utc>>` - DateTime object if conversion succeeds, None otherwise
pub fn timespec_to_datetime_utc(stamp: &r2r::unitree_go::msg::TimeSpec) -> Option<DateTime<Utc>> {
    DateTime::from_timestamp(stamp.sec as i64, stamp.nanosec)
}

/// Get current UTC timestamp as formatted string
/// 
/// # Returns
/// * `String` - Current UTC time formatted as "%Y-%m-%d %H:%M:%S%.3f UTC"
pub fn current_utc_string() -> String {
    Utc::now().format("%Y-%m-%d %H:%M:%S%.3f UTC").to_string()
}

/// Calculate time difference between two TimeSpec messages in milliseconds
/// 
/// # Arguments
/// * `stamp1` - First TimeSpec (later time)
/// * `stamp2` - Second TimeSpec (earlier time)
/// 
/// # Returns
/// * `Option<i64>` - Time difference in milliseconds, None if conversion fails
pub fn timespec_diff_ms(stamp1: &r2r::unitree_go::msg::TimeSpec, stamp2: &r2r::unitree_go::msg::TimeSpec) -> Option<i64> {
    let dt1 = timespec_to_datetime_utc(stamp1)?;
    let dt2 = timespec_to_datetime_utc(stamp2)?;
    Some(dt1.signed_duration_since(dt2).num_milliseconds())
}